#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

struct Coordinate {
    double x;
    double y;
    double yaw;
};

std::vector<Coordinate> parseCoordinates(const std::string& filename)
{
    std::vector<Coordinate> coordinates;
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件！" << std::endl;
        return coordinates;
    }

    std::string line;
    while (std::getline(file, line)) {
        std::istringstream iss(line);
        int index;
        double col1, col2, col3, col4;

        // 跳过第一列
        iss >> index;

        // 读取第二列 (x)
        iss >> col1;

        // 读取第三列 (y)
        iss >> col2;

        // 跳过第四列
        iss >> col3;

        // 读取第五列 (yaw)
        iss >> col4;

        Coordinate coord;
        coord.x = col1;
        coord.y = col2;
        coord.yaw = col4;
        coordinates.push_back(coord);
    }

    file.close();
    return coordinates;
}

int main(int argc, char* argv[])
{
    std::string filename;
    if (argc > 1) {
        filename = argv[1];
    } else {
        std::cerr << "未提供文件名，使用默认文件名 data.txt" << std::endl;
        filename = "data.txt";
    }

    std::vector<Coordinate> coords = parseCoordinates(filename);

    if (coords.empty()) {
        std::cerr << "未解析到有效坐标数据。" << std::endl;
    } else {
        for (const auto& coord : coords) {
            std::cout << "x: " << coord.x << ", y: " << coord.y << ", yaw: " << coord.yaw << std::endl;
        }
    }

    return 0;
}